import numpy as np

l1 = 0.08
l2 = 0.0315


def ImpedanceController(target_theta1,target_theta2, current_theta1, current_theta2, current_theta1_velocity,current_theta2_velocity, D=1, K=1):
    """阻抗控制器"""
    theta_error = np.array([target_theta1 - current_theta1, target_theta2 - current_theta2])
    print("角度误差:", theta_error)
    # 计算速度误差
    target_velocity = np.array([0, 0])  # 目标速度为零
    current_velocity = np.array([current_theta1_velocity, current_theta2_velocity])
    print("当前速度:", current_velocity)
    velocity_error = target_velocity - current_velocity
    print("速度误差:", velocity_error)
    # 计算力
    force = K * theta_error + D * velocity_error
    print("计算的力:", force)



    return force[0], force[1]  # 返回关节力矩

if __name__ == "__main__":
    # 测试代码
    target_theta1 = np.pi / 4
    target_theta2 = np.pi / 4
    current_theta1 = np.pi / 6
    current_theta2 = np.pi / 6
    current_theta1_velocity = 0.1
    current_theta2_velocity = 0.1

    joint_velocity = ImpedanceController(target_theta1, target_theta2, current_theta1, current_theta2, current_theta1_velocity, current_theta2_velocity)
    print("关节速度:", joint_velocity)